
#include <math.h>
#include "types.h"
#include "state.h"
#include "config.h"
#include "qmath.h"

q_t accPitch;
q_t accRoll;
q_t weightAcc;
q_t weightGyro;
q_t gyroPitch;
q_t gyroRoll;
q_t gravityLength;




#define rollover(var) { if (var < -180*Q_ONE) var += 360*Q_ONE; else if (var >= 180*Q_ONE) var -= 360*Q_ONE; }

void complementary()
{
	weightAcc = CONFIG.imu.complementaryAccWeight;

	// calculate pitch/roll from accelerometer
	if (qAbs(STATE.imu.accFilteredTransposed.z) < FLOAT_TO_Q(0.2f) &&
		qAbs(STATE.imu.accFilteredTransposed.x) < FLOAT_TO_Q(0.2f))
		weightAcc = 0;
	else
		accPitch = qMul(q32atan2(STATE.imu.accFilteredNormalised.x,
					qSqrt(
							qPow2(STATE.imu.accFilteredNormalised.y) +
							 qPow2(STATE.imu.accFilteredNormalised.z)
						)),
					Q_RAD_TO_DEG);

	rollover(accPitch);

	if (qAbs(STATE.imu.accFilteredTransposed.z) < FLOAT_TO_Q(0.2f) &&
		qAbs(STATE.imu.accFilteredTransposed.y) < FLOAT_TO_Q(0.2f))
        weightAcc = 0;
	else
		accRoll = qMul(q32atan2(STATE.imu.accFilteredNormalised.y,-STATE.imu.accFilteredNormalised.z), Q_RAD_TO_DEG);

	rollover(accRoll);

	/*
	if (imu.accPitch > 70 || imu.accPitch < -70 ||
		imu.accRoll > 70 || imu.accRoll < -70)
		weightAcc = 0;
	else if (fabs(imu.accPitch - imu.pitch - imu.gyroPitch) > 5 || fabs(imu.accRoll - imu.roll - imu.gyroRoll) > 5)
	{
		if (gravityLength > 0.8 && gravityLength < 1.2)
		{
			weightAcc *= (fabs(imu.accPitch - imu.pitch) + fabs(imu.accRoll - imu.roll)) / 4;
			if (weightAcc > filterComp.accWeight * 10) weightAcc = filterComp.accWeight * 10;
			if (weightAcc > 1) weightAcc = 1;
		}
	}*/
	gravityLength = vectorLength_inline(STATE.imu.accFilteredScaled);

	if (gravityLength < FLOAT_TO_Q(0.5f) || gravityLength > FLOAT_TO_Q(1.5))
		weightAcc = 0;
	//if (imu.roll > 60 || imu.roll < -60 || imu.pitch > 60 || imu.pitch < -60)
	//	weightAcc = 0;
	weightGyro = Q_ONE - weightAcc;

	gyroPitch = STATE.imu.gyroScaledToSec.x + STATE.imu.output.pitch;
	rollover(gyroPitch);
	if (gyroPitch - accPitch > FLOAT_TO_Q(160.0f)) gyroPitch += FLOAT_TO_Q(360.0f);
	if (gyroPitch - accPitch < FLOAT_TO_Q(-160.0)) gyroPitch -= FLOAT_TO_Q(360.0f);
	STATE.imu.output.pitch = qMul(weightAcc,accPitch) + qMul(weightGyro,gyroPitch);
	rollover(STATE.imu.output.pitch);

	gyroRoll = STATE.imu.gyroScaledToSec.y + STATE.imu.output.roll;
	rollover(gyroRoll);
	if (gyroRoll - accRoll > FLOAT_TO_Q(160.0f)) gyroRoll += FLOAT_TO_Q(360.0);
	if (gyroRoll - accRoll < FLOAT_TO_Q(-160.0f)) gyroRoll -= FLOAT_TO_Q(360.0);
	STATE.imu.output.roll = qMul(weightAcc, accRoll) + qMul(weightGyro, gyroRoll);
	rollover(STATE.imu.output.roll);

	STATE.imu.output.yaw -= STATE.imu.gyroScaledToSec.z;
	STATE.imu.output.yawD = -STATE.imu.gyroScaledToSec.z;
	STATE.imu.output.rollD = STATE.imu.gyroScaledToSec.y;
	STATE.imu.output.pitchD = STATE.imu.gyroScaledToSec.x;
}

